#include "task_ins.h"
#include "FreeRTOS.h"
#include "task.h"
#include "cmsis_os.h"
#include "usart.h"
#include "mpu9250.h"
#include "runge_kutta.h"

#define TASK_PERIOD pdMS_TO_TICKS(1) // 设置周期为1ms

uint8_t ak8963_WhoAmI = 0;
uint8_t mpu9250_WhoAmI = 0;
void INS_Zero_Calibrate();

void INS_Task(void const *argument)
{
    TickType_t xLastWakeTime;
    xLastWakeTime = xTaskGetTickCount(); // 初始化上次唤醒时间为当前时间

    MPU9250_Init(&mpu9250);
    // 初始化MPU9250
    HAL_GPIO_WritePin(GPIOA,GPIO_PIN_6,GPIO_PIN_RESET);
    //ak8963_WhoAmI = mpu_r_ak8963_WhoAmI(&mpu9250);// 读取AK8963的WhoAmI
    mpu9250_WhoAmI = mpu_r_WhoAmI(&mpu9250);// 读取MPU9250的WhoAmI

    INS_Zero_Calibrate();
    for (;;)
    {
        /*Task code here*/
    // MPU9250_ReadAccel(&mpu9250);
    // MPU9250_ReadGyro(&mpu9250);
        HAL_GPIO_WritePin(GPIOA,GPIO_PIN_5,GPIO_PIN_RESET);
        MPU9250_Read_ALL(&mpu9250);

        imu.q=updateAttitude(imu.q,mpu9250.mpu_data.Accel[0],mpu9250.mpu_data.Accel[1],mpu9250.mpu_data.Accel[2],\
        mpu9250.mpu_data.Gyro[0],mpu9250.mpu_data.Gyro[1],mpu9250.mpu_data.Gyro[2],0,0,0,0.001);
        HAL_GPIO_WritePin(GPIOA,GPIO_PIN_5,GPIO_PIN_SET);

        /*Task code end*/
        vTaskDelayUntil(&xLastWakeTime, TASK_PERIOD); // 绝对延时
    }
}


/**
 * @brief 传感器零偏校准
 * 
 * */
void INS_Zero_Calibrate()
{
    #define CALIBRATION_COUNT 100 // 校准次数

    MPU9250 mpu_buff[CALIBRATION_COUNT];
    float ax_mean = 0.0, ay_mean = 0.0, az_mean = 0.0;
    float wx_mean = 0.0, wy_mean = 0.0, wz_mean = 0.0;
    while(1)
    {
        for (int i = 0; i < CALIBRATION_COUNT; i++)
        {
            MPU9250_Read_ALL(&mpu_buff[i]);
        }
        // 计算加速度的均值

        for (int i = 0; i < CALIBRATION_COUNT; i++)
        {
            ax_mean += mpu_buff[i].mpu_prop.Accel_row[0];
            ay_mean += mpu_buff[i].mpu_prop.Accel_row[1];
            az_mean += mpu_buff[i].mpu_prop.Accel_row[2];
        }
        ax_mean /=CALIBRATION_COUNT;
        ay_mean /= CALIBRATION_COUNT;
        az_mean /= CALIBRATION_COUNT;

        // 计算加速度的方差
        float ax_variance = 0.0, ay_variance = 0.0, az_variance = 0.0;
        for (int i = 0; i < CALIBRATION_COUNT; i++)
        {
            ax_variance += (mpu_buff[i].mpu_prop.Accel_row[0] - ax_mean) * (mpu_buff[i].mpu_prop.Accel_row[0] - ax_mean);
            ay_variance += (mpu_buff[i].mpu_prop.Accel_row[1] - ay_mean) * (mpu_buff[i].mpu_prop.Accel_row[1] - ay_mean);
            az_variance += (mpu_buff[i].mpu_prop.Accel_row[2] - az_mean) * (mpu_buff[i].mpu_prop.Accel_row[2] - az_mean);
        }
        ax_variance /= CALIBRATION_COUNT;
        ay_variance /= CALIBRATION_COUNT;
        az_variance /= CALIBRATION_COUNT;

        // 计算角速度的均值

        for (int i = 0; i < CALIBRATION_COUNT; i++)
        {
            wx_mean += mpu_buff[i].mpu_prop.Gyro_row[0];
            wy_mean += mpu_buff[i].mpu_prop.Gyro_row[1];    
            wz_mean += mpu_buff[i].mpu_prop.Gyro_row[2];
        }
        wx_mean /= CALIBRATION_COUNT;
        wy_mean /= CALIBRATION_COUNT;
        wz_mean /= CALIBRATION_COUNT;

        // 计算角速度的方差
        float wx_variance = 0.0, wy_variance = 0.0, wz_variance = 0.0;
        for (int i = 0; i < CALIBRATION_COUNT; i++)
        {
            wx_variance += (mpu_buff[i].mpu_prop.Gyro_row[0] - wx_mean) * (mpu_buff[i].mpu_prop.Gyro_row[0] - wx_mean);
            wy_variance += (mpu_buff[i].mpu_prop.Gyro_row[1] - wy_mean) * (mpu_buff[i].mpu_prop.Gyro_row[1] - wy_mean);
            wz_variance += (mpu_buff[i].mpu_prop.Gyro_row[2] - wz_mean) * (mpu_buff[i].mpu_prop.Gyro_row[2] - wz_mean);
        }    
        wx_variance /= CALIBRATION_COUNT;
        wy_variance /= CALIBRATION_COUNT;
        wz_variance /= CALIBRATION_COUNT;

        if((wx_variance+wy_variance+wz_variance)<5)
        {
            break;
        }

    }

    zero_bais.acc_bias[0]=ax_mean;
    zero_bais.acc_bias[1]=ay_mean;
    zero_bais.acc_bias[2]=az_mean;

    zero_bais.gyro_bias[0]=wx_mean;
    zero_bais.gyro_bias[1]=wy_mean;
    zero_bais.gyro_bias[2]=wz_mean;
    
} 